#ifndef SLAM_ROS_MAP_EDITOR_H
#define SLAM_ROS_MAP_EDITOR_H

#include <cstdio>
#include <memory>
#include <string>
#include <vector>
#include <map>

#include "rclcpp/rclcpp.hpp"
#include "yaml-cpp/yaml.h"
#include "absl/synchronization/mutex.h"
#include "Eigen/Core"
#include "Eigen/Geometry"

#include "slam_ros/ros_log_sink.h"
#include "slam_ros/node_constants.h"
#include "slam/io/internal/pbstream_info.h"
#include "slam/io/proto_stream_deserializer.h"
#include "slam/transform/rigid_transform.h"
#include "slam_ros/map_builder_bridge.h"
#include "slam/mapping/pose_graph_data.h"

#include "agv_srvs/srv/edit_map.hpp"
#include "slam_ros_msgs/msg/mapdata_progress.hpp"
#include "agv_msgs/msg/map_edit_poly.hpp"

namespace slam_ros {

using namespace slam;
using namespace slam::io;
using namespace slam::mapping::proto;
using namespace slam::mapping;
using ::slam::transform::Rigid3d;

enum EditAttribute {
    EARSE = 0,
    ADD   = 1
};

struct MapEditT {
    bool operate = false;
    double angle = 0.0;
    double x = 0.0;
    double y = 0.0;
};

struct MapEditEarse {
    bool operate = false;
    std::vector<agv_msgs::msg::MapEditPoly> map_edit_poly;
};

struct MapEditOperation {
    std::string map_id;
    MapEditT agv_map_edit_t;
    MapEditEarse agv_map_edit_earse;
};

class MapEditor : public rclcpp::Node {
public:
    explicit MapEditor(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
    ~MapEditor() override = default;
    
    MapEditor(const MapEditor&) = delete;
    MapEditor& operator=(const MapEditor&) = delete;

private:
    bool handleEditMap(
        const std::shared_ptr<agv_srvs::srv::EditMap::Request> request,
        std::shared_ptr<agv_srvs::srv::EditMap::Response> response);
    
    bool AutoHandleEditMap(
        const std::shared_ptr<agv_srvs::srv::EditMap::Request> request,
        std::shared_ptr<agv_srvs::srv::EditMap::Response> response);
    void publishEditMapProgress();
    bool loadRtYaml(const std::string& file_name);
    bool loadMapYaml(const std::string& file_name);
    void writeOut(
        const std::string& pbstream_filename, 
        const std::string& map_filestem,
        double resolution, 
        double trans_x, double trans_y, 
        double rot_w, double rot_x, double rot_y, double rot_z);
    
    void convertToImgWithoutLocation(
        const std::string& pbstream_filename, 
        const std::string& map_filestem,
        double resolution);
    
    void eraseOriginalMap(
        const std::string& mode, 
        const std::string& map_id,
        int rect_leftop_x, int rect_leftop_y,
        int erase_rect_width, int erase_rect_height,
        int width, int height);

    rclcpp::Service<agv_srvs::srv::EditMap>::SharedPtr edit_map_service_;
    rclcpp::Publisher<slam_ros_msgs::msg::MapdataProgress>::SharedPtr edit_map_progress_pub_;
    rclcpp::TimerBase::SharedPtr progress_timer_;

    Rigid3d map_init_pose_;
    Eigen::Vector3d map_init_translation_;
    Eigen::Quaterniond map_init_quaternion_;
    double map_init_yaw_;
    
    MapEditOperation map_edit_operation_;
    mutable absl::Mutex mutex_;
    PoseGraphData data_ GUARDED_BY(mutex_);
    ValueConversionTables conversion_tables_;
    slam_ros_msgs::msg::MapdataProgress edit_map_progress_msg_;

    static const std::string match_file_path_;
    static const std::string map_path_;
};

}  // namespace slam_ros

#endif  // SLAM_ROS_MAP_EDITOR_H